package edu.tec.mty.core.modules.models;

public class Position {

	private double x;
	private double y;
	private double bodyAngle;
	private double neckAngle;
	
	public Position(double x, double y) {
		this.x = x;
		this.y = y;
		this.bodyAngle = 0;
		this.neckAngle = 0;
	}
	
	public Position(double x, double y, double bodyAngle) {
		this.x = x;
		this.y = y;
		this.bodyAngle = bodyAngle;
		this.neckAngle = 0;
	}
	
	public Position(double x, double y, double bodyAngle, double neckAngle) {
		this.x = x;
		this.y = y;
		this.bodyAngle = bodyAngle;
		this.neckAngle = neckAngle;
	}
	
	public double getX() {
		return this.x;
	}
	
	public double getY() {
		return this.y;
	}
	
	public double getBodyAngle() {
		return this.bodyAngle;
	}
	
	public double getNeckAngle() {
		return this.neckAngle;
	}
	
	public double getDistanceTo(Position position) {
		return Math.sqrt(Math.pow(this.x - position.getX(), 2.0) + Math.pow(this.y - position.getY(), 2.0));
	}
	
	public double getAngleFromBodyTo(Position position) {
		double angle = Math.toDegrees(Math.atan2(position.getY() - this.y, position.getX() - this.x)) - this.bodyAngle;
		if (angle > 180.0) {
			angle -= 360.0;
		}
		if (angle < -180.0) {
			angle += 360.0;
		}
		return angle;
	}
	
	/*public double getAngleFromNeckTo(Position position) {
		return Math.toDegrees(Math.atan2(position.getY() - this.y, position.getX() - this.x)) - this.bodyAngle;
	}*/
	
	//Test
	
	/*public static void main(String[] args) {
		Position position = new Position(-10.0, 0.0, 0.0);
		System.out.println(position.getAngleFromBodyTo(new Position(1.0, -1.732)));
	}/**/
	
}
